﻿/*
 * Created by SharpDevelop.
 * User: Eddie
 * Date: 4/14/2012
 * Time: 11:35 PM
 * 
 * To change this template use Tools | Options | Coding | Edit Standard Headers.
 */
using System;
using System.Drawing;
using System.IO;

namespace PathFinder
{
    /* @class 
     * ShortestPath
     * 
     * @brief 
     * Finds the shortest path between 2 points while avoiding obstacles.
	 */
	public class ShortestPath
	{	
	    /* Finds the shortest path between two points in the map provided.
	     * 
         * @param
         * nodes A mapping of the nodes of the map that needs to be traversed.
         * 
         * @return
         * Provides the nodes again but with their values changed to reflect the shortest path.
	     */
		public Node [,] path (Node [,] nodes)
		{
		    Point start = new Point (10,0);
		    Point end = new Point (49,49);
		    nodes=initializeDistances (end, nodes);
		    return astar (start, end, nodes);
		}
		
		/* Finds the shortest straight line distance from any point in the map to the end point of the traversal.
	     * 
         * @param
         * destination The end point of the traversal.
         * nodes A mapping of the nodes of the map that needs to be traversed.
         * 
         * @return
         * Provides the nodes again but with their values changed to reflect the straight line distances from each node to the end point.
	     */
		public Node [,] initializeDistances (Point destination, Node [,] nodes)
		{
		    for (int i=0; i<nodes.GetLength(0);i++)
		    {
		        for (int j=0; j<nodes.GetLength(1);j++)
		        {
		            if (nodes [i,j].traverse == true)
		            {
		                nodes [i,j].distance= distance (new Point (i,j), destination);
		            }
		        }
		    }
		    return nodes;
		}

		/* Based on the nodes visited in order to arrive from the start to destination points, this method back tracks from the end
		 * point back to the start point in order to find the shortest path.
	     * 
         * @param
         * start The start point of the traversal.
         * end The end point of the traversal.
         * nodes A mapping of the nodes of the map that needs to be traversed.
         * 
         * @return
         * Provides the nodes again but with their values changed to reflect the shortest path between start and end.
	     */
		public Node [,] findPath (Point start, Point end, Node [,] nodes)
		{
		    var next=nodes [end.X, end.Y].totalDistance-1; // the next distance from the start point
		    
		    // once we reach the start point
		    if (end == start)
		    {
		        return nodes;
		    }
		    else
		    {
		        for (int i=-1; i<2;i++)
		        {
		            for (int j=-1; j<2;j++)
		            {
		                // if the point is not in the map
		                if (end.X+i<0 || end.X+i>=nodes.GetLength(0) ||
		                    end.Y+j<0 || end.Y+j>=nodes.GetLength(1))
		                {
		                    continue;
		                }
		                
		                // if this node has the same distance value as what the next node should have
		                if (nodes [end.X+i,end.Y+j].totalDistance==next && nodes [end.X+i,end.Y+j].visited==true)
		                {
		                    nodes [end.X+i,end.Y+j].path = true;
		                    return findPath (start, new Point (end.X+i, end.Y+j), nodes);
		                }
		            }
		        }
		    }
		    return null;
		}
		
		/* Uses A* algorithm to find the shortest path between two points
	     * 
         * @param
         * start The start point of the traversal.
         * end The end point of the traversal.
         * nodes A mapping of the nodes of the map that needs to be traversed.
         * 
         * @return
         * Provides the nodes again but with their values changed to reflect the shortest path between start and end.
	     */
		public Node [,] astar (Point start, Point end, Node [,] nodes)
		{
		    int currentDist;                  // the travel distance at the current node
		    double tentativeDist;             // the tentative straight line distance
		    Point currentPoint = start;       // the current point being traversed
		    
            while (traversedAll (nodes) == false)
		    {
                // if the destination has been reached
		        if (currentPoint == end)
		        {
		            Console.WriteLine ("found end"); 
		            return findPath (start, end, nodes);     //shortest path
		        }
		        
		        nodes [currentPoint.X, currentPoint.Y].visited = true;                // mark the current node as visited
		        currentDist = nodes [currentPoint.X, currentPoint.Y].totalDistance;   // update the current distance based on the current node
		        
		        // traverse through the 8 nodes adjacent to the current node
		        for (int i=-1; i<2;i++)
		        {
		            for (int j=-1; j<2;j++)
		            {
		                // if the current node is not on the data map, has been visited already, or is not traverseable
		                if (currentPoint.X+i<0 || currentPoint.X+i>=nodes.GetLength(0) ||
		                    currentPoint.Y+j<0 || currentPoint.Y+j>=nodes.GetLength(1) ||
		                    nodes[currentPoint.X+i,currentPoint.Y+j].visited == true ||
		                    nodes[currentPoint.X+i,currentPoint.Y+j].traverse == false)
		                {
		                    
		                    continue;
		                }
		                
		                tentativeDist=nodes [currentPoint.X+i, currentPoint.Y+j].distance;    // update to the new straight line distance
		                
		                // if the tentative distance is shorter and it's neighbour has not been visited already
		                if (tentativeDist<nodes [currentPoint.X,currentPoint.Y].distance && nodes [currentPoint.X+i, currentPoint.Y+j].visitedNeighbour == false)
		                {
		                    nodes [currentPoint.X+i,currentPoint.Y+j].visitedNeighbour = true;
		                    nodes [currentPoint.X+i,currentPoint.Y+j].totalDistance = currentDist+1;
		                }
		            }
		        }
		        
		        // move on to the next point
		        currentPoint=nextPoint (nodes);
		        
		        // if we can't find a new point
		        if (currentPoint == new Point (-1,-1))
		        {
		            Console.WriteLine (":(");
		            break;
		        }
		    }
		    return nodes;
		}
		
		/* Finds the straight line distance between two points
	     * 
         * @param
         * p1 Point 1.
         * p2 Point 2.
         * 
         * @return
         * Provides the straight line distance betwee points 1 and 2.
	     */
		public double distance (Point p1, Point p2)
		{
		    int width = (int) (Math.Pow(p1.X-p2.X,2));    // width for pythagoerean theorem
		    int height = (int) (Math.Pow(p1.Y-p2.Y,2));   // height for pythagoerean theorem
		   
		    double distance = (double) Math.Sqrt (width + height);
		    return distance;
		}
		
		/* Checks if all of the nodes have been traversed in a map of nodes.
	     * 
         * @param
         * nodes A mapping of the nodes of the map that needs to be checked.
         * 
         * @return
         * Whether if all of the nodes in a node map have been traversed.
	     */
		public bool traversedAll (Node [,] nodes)
		{
		    for (int i=0; i<nodes.GetLength(0);i++)
		    {
		        for (int j=0; j<nodes.GetLength(1);j++)
		        {
		            if (nodes [i,j].visited == false)
		            {
		                return false;
		            }
		        }
		    }
		    return true;
		}
		
		/* Finds the next node that should be visited in the A* algorithm. This point is mostly selected based on the straight line distances.
	     * 
         * @param
         * nodes A mapping of the nodes of the map that needs to be traversed.
         * 
         * @return
         * Provides the location of the next point to visit.
	     */
		public Point nextPoint (Node [,] nodes)
		{
		    double distance = -1;         // current shortest available straight line path
		    Point p = new Point ();       // new point to visit

		    for (int i=0; i<nodes.GetLength (0);i++)
		    {
		        for (int j=0; j<nodes.GetLength (1);j++)
		        {
		            if (nodes [i,j].visitedNeighbour == true && nodes [i,j].visited == false && nodes [i,j].traverse == true &&
		                (distance==-1 || nodes[i,j].distance < distance))
		            {
		                distance = nodes[i,j].distance;
		                p =  new Point (i,j);
		            }
		        }
		    }

		    // if no new point can be found
		    if (p == new Point ())
		    {
		        p=new Point (-1,-1);
		    }
		    return p;
		}
	}
}